/* 
 * File:   generateMap.cpp
 * Author: hadrien
 *
 * Created on 12 mars 2012, 14:31
 */

#include <cstdlib>
#include "mrpt/base.h"
#include "mrpt/utils.h"
#include "mrpt/slam.h"
using namespace std;
using namespace mrpt::poses;
using namespace mrpt::random;
using namespace mrpt::slam;

/*
 * 
 */
int main(int argc, char** argv) {
    CLandmarksMap landmarkMap;
    if(argc < 2) return 1;
    
    int uniqueIds = 1;
    for (size_t i = 0; i < 70; i++) {
        CLandmark LM;
        CPointPDFGaussian pt3D;

        // Random coordinates:
        pt3D.mean = CPoint3D(
                             randomGenerator.drawUniform(-15, 15),
                             randomGenerator.drawUniform(-15, 15),
                             0);

        // Add:
        LM.createOneFeature();
        LM.features[0]->type = featBeacon;
        LM.ID = uniqueIds++;
        LM.setPose(pt3D);

        landmarkMap.landmarks.push_back(LM);
    }
    string file(argv[1]);
    landmarkMap.saveToTextFile(file);
    return 0;
}

